#include "eigen/position_decay_linear.h"
#include "log.h"

namespace  ldl_eigen
{
PositionDecayLinear::PositionDecayLinear(int64_t d, int64_t h, int64_t width, int64_t height) : Linear(d, h)
{
    m_width = width;
    m_height = height;

    m_weights_deacy.resize(d, h);
    for(int64_t h_index = 0; h_index < h; h_index++)
    {
        // auto h_index_relative = ((double)h_index/h)*d;
        auto h_d_relative = std::sqrt(d)/std::sqrt(h);
        int64_t h_row = h_index / (m_width / h_d_relative);
        int64_t h_col = h_index - h_row * (m_width / h_d_relative);
        Eigen::Vector2f h_point{h_row * h_d_relative, h_col * h_d_relative};
        // LogInfo() << "h_d_relative: \n" << h_d_relative;
        // LogInfo() << "h_point: \n" << h_point;
        for(int64_t d_index = 0; d_index < d; d_index++)
        {
            int64_t d_row = d_index / m_width;
            int64_t d_col = d_index - d_row * m_width;

            Eigen::Vector2f d_point{d_row, d_col};
            m_weights_deacy(d_index, h_index) = (distance(h_point, d_point)+0.1) * 1e-7;
        }
    }
}
float PositionDecayLinear::distance(Eigen::Vector2f a, Eigen::Vector2f b)
{
    return (a - b).norm();
}

void PositionDecayLinear::update()
{
    const double lr = 0.01;
    m_weights -= lr * m_weights_gradient;
    m_weights -= ((m_weights_deacy.array() * m_weights.array()).matrix());
    m_bias -= lr * m_bias_gradient;
}

const Eigen::MatrixXf& PositionDecayLinear::weights_deacy()
{
    return m_weights_deacy;
}
}